#include "twowheels.h"

TwoWheels::TwoWheels()
{
}

void TwoWheels::setParameters(QList<double> params)
{
    xMax = params[0];
    yMax = params[1];
    diameter1 = params[2];
    diameter2 = params[3];
    spacing = params[4];
    x = 0;
    y = 0;
    theta = 0;
    prev1 = prev2 = 0;
}

void TwoWheels::addMotor(int freq, double delay, double tau, double imprecision)
{
    motors.append(new ContinuousMotor(this, motors.size(), freq, delay, tau, imprecision));
}

void TwoWheels::addSensor(int freq, double delay, double imprecision)
{
    sensors.append(new MotorStateSensor(this, sensors.size(), freq, delay, imprecision));
}

void TwoWheels::checkValue(int id)
{
    if(id == 0)
    {
        return;
    }

    double sum = (((M_PI/180.)*motors[0]->getState() - prev1)*diameter1 + ((M_PI/180.)*motors[1]->getState() - prev2)*diameter2)/4;

    double newX = sum*cos(theta) + x;
    double newY = sum*sin(theta) + y;

    if( (newX > xMax) || (newY > yMax) || (newX < 0) || (newY < 0) )
    {
        motors[0]->setState(prev1);
        motors[1]->setState(prev2);
    }
    else
    {
        double diff = (((M_PI/180.)*motors[1]->getState() - prev2)*diameter2 - ((M_PI/180.)*motors[1]->getState() - prev1)*diameter1)/2;

        prev1 = motors[0]->getState();
        prev2 = motors[1]->getState();

        x = newX;
        y = newY;

        theta += diff/spacing;
    }
}

QList<double> TwoWheels::getPosition()
{
    QList<double> result;
    result << x << y << theta;
    return result;
}


void TwoWheels::setPosition(double a_x, double a_y, double a_theta)
{
    x = a_x;
    y = a_y;
    theta = a_theta;
}

QList<double> TwoWheels::getPositionNormee()
{
    QList<double> result;
    result << x/xMax << y/yMax << theta;
    return result;
}

double TwoWheels::getMotorStateNormee(int id)
{
    double thetaMax = 0.0;
    if(id == 0)
        thetaMax = (xMax*cos(theta)+yMax*sin(theta))*2*180./(M_PI*diameter1);
    else
        thetaMax = (xMax*sin(theta)-yMax*cos(theta))*2*180./(M_PI*diameter2);

    return  motors[id]->getState()/thetaMax;
}

